/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : sliding_window.h
 *@brief   : sliding window pipeline
 *@authors : zhanglei, zhanglei_723@126.com
 *@version : v1.0
 *@data    : 2025/8/24
 *
 */
#ifndef SLIDING_WINDOW_H_
#define SLIDING_WINDOW_H_

#include <Eigen/Core>
#include <memory>

#include "common/map_builder.h"
#include "common/make_unique.hpp"

#include "feature/feature_matcher.h"
#include "triangulator/triangulate.h"

#include "optimizer/sliding_window_optimizer.h"

#include "publisher/publisher.h"

namespace mapping {

using namespace frame;
using namespace preintegration;
using namespace feature;
using namespace common;
using namespace triangulator;
using namespace optimizer;

using namespace publisher;

class SlidingWindow {
public:
    SlidingWindow(const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const std::shared_ptr<Publisher> publisher_ptr);
    ~SlidingWindow();
    void StartMapping(const ImuData& imu_data, const WheelData& wheel_data, const std::vector<FeaturePoint>& feature_data);
private:
    void Initialize(const ImuData& imu_data, const WheelData& wheel_data, const std::vector<FeaturePoint>& feature_data);
    bool ImuBiasEstimate(Eigen::Vector3d& imu_bias, const WheelData& wheel_data, const ImuData& imu_data);
    Eigen::Vector3d TransformImuData(const ImuData& imu_data);
    Eigen::Vector3d TransformWheelData(const WheelData& wheel_data);
    // feature::FeaturePoint TransformFeatureData(const FeaturePoint& feature_data);

    void StartProcessSliddingWindow(const ImuData& imu_data, const WheelData& wheel_data, const std::vector<FeaturePoint>& feature_data);
    void SliddingWindow(const ImuData& imu_data, const WheelData& wheel_data, const std::vector<FeaturePoint>& feature_data);

    void FeatureTriangulate();
    void Optimizer(const float& velocity);
    Eigen::Matrix4d extrinsic_matrix_;

    bool is_init_;

    std::vector<ImuData> imu_data_;
    Eigen::Vector3d imu_init_bias_;

    ImuOptions imu_options_;
    WheelOptions wheel_options_;
    
    std::shared_ptr<ImuPreIntegration> imu_preintegration_ptr_;
    std::shared_ptr<WheelPreIntegration> wheel_preintegration_ptr_;

    State state_;  

    Eigen::Vector3d last_imu_;
    Eigen::Vector3d last_odometry_pose_;

    float last_imu_timestamp_;
    SE2 last_wheel_pose_;
    
    std::shared_ptr<FeatureDatabase> feature_database_ptr_;

    std::unique_ptr<FeatureMatcher> feature_matcher_ptr_;

    std::unique_ptr<Triangulate> triangulate_ptr_;

    uint8_t camera_id_; 
    uint64_t frame_id_; 
    uint64_t feature_id_;

    std::map<uint64_t, Eigen::Matrix4d> poses_;

    std::shared_ptr<Frame> frame_ptr_;
    std::unique_ptr<SlidingWindowOptimizer> sliding_window_optimizer_ptr_;

    std::shared_ptr<Publisher> publisher_ptr_;

};

}

#endif  // LOCAL_MAPPING_H_
